package com.hongji.lms5xxcommunicator.scanner.imp;

import com.hongji.lms5xxcommunicator.realtime.sync.XSource;
import com.hongji.lms5xxcommunicator.scanner.ScanFrame;
import com.hongji.lms5xxcommunicator.scanner.ScanStrategy;
import com.hongji.lms5xxcommunicator.scanner.config.LivoxConfig;
import com.hongji.lms5xxcommunicator.scanner.config.LivoxConfig.MountSide;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.core.ResolvableType;
import org.springframework.core.env.Environment;
import org.springframework.stereotype.Service;

import java.lang.reflect.Field;
import java.lang.reflect.Method;
import java.util.*;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.CopyOnWriteArrayList;
import java.util.concurrent.atomic.AtomicBoolean;

import org.springframework.boot.context.properties.bind.Bindable;
import org.springframework.boot.context.properties.bind.Binder;

/**
 * 多 Livox 管理
 * - 每台设备按自身 ROI（x0/x1, y0/y1, z0/z1 任意维度可选）过滤；
 * - 汇聚层对所有设备结果做并集（不去重、不融合）后推送；
 * - 逐台支持完整“单机等价”配置（旋转/比例/翻转/偏移/PLC 等）。
 */
@Slf4j
@Service
public class MultiLivoxScannerStrategy implements ScanStrategy {

    private final Environment env;
    private final XSource xSource;

    private final AtomicBoolean running = new AtomicBoolean(false);
    private volatile ScanFrameListener listener;

    private final List<LivoxMid360ScannerStrategy> children = new CopyOnWriteArrayList<>();

    /** 将时间接近的帧打包（毫秒） */
    private int pairToleranceMs;

    /** 每设备最近一帧（用于成组发射） */
    private static final class Pending {
        int devIdx;
        long hostNs;
        double[] X, Y, Z;
    }
    private final Map<Integer, Pending> lastFrames = new HashMap<>();

    /** 设备级 ROI（用于拼接） */
    private static final class Roi {
        double x0 = Double.NEGATIVE_INFINITY, x1 = Double.POSITIVE_INFINITY;
        double y0 = Double.NEGATIVE_INFINITY, y1 = Double.POSITIVE_INFINITY;
        double z0 = Double.NEGATIVE_INFINITY, z1 = Double.POSITIVE_INFINITY;
        boolean hasX = false, hasY = false, hasZ = false;
        @Override public String toString() {
            return String.format(Locale.ROOT,
                    "Roi{x=[%s,%s], y=[%s,%s], z=[%s,%s]}",
                    hasX?fmt(x0):"-inf", hasX?fmt(x1):"+inf",
                    hasY?fmt(y0):"-inf", hasY?fmt(y1):"+inf",
                    hasZ?fmt(z0):"-inf", hasZ?fmt(z1):"+inf");
        }
        private static String fmt(double v){ return String.format(Locale.ROOT,"%.3f", v); }
    }
    private final Map<Integer, Roi> roiPerDevice = new ConcurrentHashMap<>();

    @Autowired
    public MultiLivoxScannerStrategy(Environment env, XSource xSource) {
        this.env = env;
        this.xSource = xSource;
    }

    @Override
    public void start(ScanFrameListener listener) throws Exception {
        if (!running.compareAndSet(false, true)) return;
        this.listener = listener;

        // 统一托管 XSource
        xSource.start();

        // 基础配置（默认值）
        LivoxConfig base = buildBaseCfg().recomputeRotation();

        // 拼接帧对齐容忍
        this.pairToleranceMs = getInt("scanner.LivoxMulti.pair_tolerance_ms", 20);

        // 设备列表
        List<Map<String, Object>> devices = readDevicesList();

        if (devices == null || devices.isEmpty()) {
            // 单设备：用 base
            LivoxConfig c = cloneCfg(base);
            c.manageXSourceLifecycle = false;

            // ★ 补丁：与单机等价 — 将 coord.y_base 叠加到 yOffset
            c.yOffset += c.yBase;
            c.yBase = 0;

            c.recomputeRotation();

            Roi r0 = new Roi(); // 默认无约束
            roiPerDevice.put(0, r0);

            LivoxMid360ScannerStrategy s = new LivoxMid360ScannerStrategy(xSource, c);
            final int idx = 0;
            s.start(f -> onSubFrame(idx, f));
            children.add(s);
            log.info("[MultiLivox] single device udpPort={}, cfg={}, roi={}", c.udpPort, c, r0);
        } else {
            int idx = 0;
            for (Map<String, Object> dev : devices) {
                LivoxConfig c = cloneCfg(base);

                // 解析设备 ROI
                Roi r = parseDeviceRoi(dev);

                // Y/Z ROI 下沉到子策略（减负载）；X ROI 在汇聚层判断
                if (r.hasY) { c.yMin = r.y0; c.yMax = r.y1; }
                if (r.hasZ) { c.zMin = r.z0; c.zMax = r.z1; }

                // 设备级覆盖（单机等价配置）
                applyDeviceOverrides(c, dev);

                // ★ 补丁：与单机等价 — 将 coord.y_base 叠加到 yOffset
                c.yOffset += c.yBase;
                c.yBase = 0;

                c.manageXSourceLifecycle = false;
                c.recomputeRotation();

                roiPerDevice.put(idx, r);

                LivoxMid360ScannerStrategy s = new LivoxMid360ScannerStrategy(xSource, c);
                final int id = idx;
                s.start(f -> onSubFrame(id, f));
                children.add(s);

                log.info("[MultiLivox] device[{}] udp={}, side={}, yaw/pitch/roll=({}/{}/{}), xOff={}, yOff={}, plcMerge={}, roi={}",
                        idx, c.udpPort, c.side, c.yawDeg, c.pitchDeg, c.rollDeg, c.xOffset, c.yOffset, c.plcMergeEnabled, r);
                idx++;
            }
            log.info("[MultiLivox] total devices: {}", children.size());
        }

        log.info("[MultiLivox] STITCH enabled: per-device ROI, pairToleranceMs={}", pairToleranceMs);
    }

    @Override
    public void stop() {
        if (!running.getAndSet(false)) return;
        for (LivoxMid360ScannerStrategy s : children) {
            try { s.stop(); } catch (Exception ignore) {}
        }
        children.clear();
        lastFrames.clear();
        roiPerDevice.clear();
        try { xSource.stop(); } catch (Exception ignore) {}
        log.info("[MultiLivox] stopped");
    }

    @Override
    public boolean isRunning() { return running.get(); }

    /* ================= 子实例回调：分组并“拼接”发射 ================= */
    private final Object mergeLock = new Object();

    private void onSubFrame(int devIdx, ScanFrame f) {
        if (f == null || listener == null) return;

        long hostNs = tryGetHostNs(f);
        double[] X = getArray(f, "X", "getX", "getXs", "getXw");
        double[] Y = getArray(f, "Y", "getY", "getYs", "getYw");
        double[] Z = getArray(f, "Z", "getZ", "getZs", "getZw");
        if (X == null || Y == null || Z == null) return;

        synchronized (mergeLock) {
            Pending cur = new Pending();
            cur.devIdx = devIdx;
            cur.hostNs = hostNs; cur.X = X; cur.Y = Y; cur.Z = Z;
            lastFrames.put(devIdx, cur);

            List<Pending> group = new ArrayList<>();
            group.add(cur);
            long tolNs = pairToleranceMs * 1_000_000L;

            for (Map.Entry<Integer, Pending> e : lastFrames.entrySet()) {
                if (e.getKey() == devIdx) continue;
                Pending p = e.getValue();
                if (p == null) continue;
                if (Math.abs(p.hostNs - hostNs) <= tolNs) group.add(p);
            }

            emitStitched(group);

            for (Pending p : group) {
                lastFrames.values().removeIf(v -> v == p);
            }
        }
    }

    /** STITCH：每设备按自身 ROI 过滤，然后简单拼接（无跨设备去重/融合） */
    private void emitStitched(List<Pending> group) {
        if (group == null || group.isEmpty()) return;

        int cap = 0;
        long hostNsMax = 0;
        for (Pending p : group) {
            cap += Math.min(p.X.length, Math.min(p.Y.length, p.Z.length));
            hostNsMax = Math.max(hostNsMax, p.hostNs);
        }
        if (cap == 0) return;

        double[] Xo = new double[cap];
        double[] Yo = new double[cap];
        double[] Zo = new double[cap];
        int n = 0;

        for (Pending p : group) {
            Roi r = roiPerDevice.getOrDefault(p.devIdx, new Roi());
            int m = Math.min(p.X.length, Math.min(p.Y.length, p.Z.length));
            for (int i = 0; i < m; i++) {
                double x = p.X[i], y = p.Y[i], z = p.Z[i];
                if (r.hasX && (x < r.x0 || x > r.x1)) continue;
                if (r.hasY && (y < r.y0 || y > r.y1)) continue;
                if (r.hasZ && (z < r.z0 || z > r.z1)) continue;
                Xo[n] = x; Yo[n] = y; Zo[n] = z; n++;
            }
        }

        if (n == 0) return;
        if (n < cap) {
            Xo = Arrays.copyOf(Xo, n);
            Yo = Arrays.copyOf(Yo, n);
            Zo = Arrays.copyOf(Zo, n);
        }
        listener.onFrame(new ScanFrame(hostNsMax, null, Xo, Yo, Zo));
    }

    /* ================= 从 Environment 读取基础 Livox 配置 ================= */
    private LivoxConfig buildBaseCfg() {
        LivoxConfig c = new LivoxConfig();
        c.udpPort      = getInt("scanner.Livox.udp.port", 56301);
        c.recvBufBytes = Math.max(getInt("scanner.Livox.udp.recv_buf_bytes", 4 * 1024 * 1024), 1 << 20);

        c.offDotNum   = getInt("scanner.Livox.offset.dot_num", 5);
        c.offDataType = getInt("scanner.Livox.offset.data_type", 10);
        c.headerBytes = getInt("scanner.Livox.header_bytes", 18);

        c.strideType1 = getInt("scanner.Livox.pt.stride.type1", 12);
        c.strideType2 = getInt("scanner.Livox.pt.stride.type2", 6);
        c.skipAfter1  = getInt("scanner.Livox.pt.skip_after_xyz.type1", 0);
        c.skipAfter2  = getInt("scanner.Livox.pt.skip_after_xyz.type2", 0);

        c.debugHexdump = getBool("scanner.Livox.debug.hexdump", false);

        c.zMin = getDouble("scanner.Livox.filter.z_min", 0);
        c.zMax = getDouble("scanner.Livox.filter.z_max", 2000);
        c.yMin = getDouble("grid.spec.y0", -1000);
        c.yMax = getDouble("grid.spec.y1", 1000);

        // 全局 coord（可被每台设备覆盖）
        c.coordEnabled = getBool("scanner.coord.enabled", true);
        c.xScale  = getDouble("scanner.coord.x_scale", 1.0);
        c.yScale  = getDouble("scanner.coord.y_scale", 1.0);
        c.zScale  = getDouble("scanner.coord.z_scale", 1.0);
        c.invertX = getBool("scanner.coord.invert_x", false);
        c.invertY = getBool("scanner.coord.invert_y", false);
        c.invertZ = getBool("scanner.coord.invert_z", false);
        c.yBase   = getDouble("scanner.coord.y_base", 0);
        c.zOffset = getDouble("scanner.coord.z_offset", 0);

        // 设备外参/姿态
        c.h0      = getDouble("scanner.Livox.h0", 0);
        c.xOffset = getDouble("scanner.Livox.x_offset", 0);
        c.yOffset = getDouble("scanner.Livox.y_offset", 0);
        c.sX      = getInt("scanner.Livox.sx", 1);
        c.sY      = getInt("scanner.Livox.sy", 1);
        c.sZ      = getInt("scanner.Livox.sz", 1);

        c.yawDeg   = getDouble("scanner.Livox.mount.yaw_deg", 0);
        c.pitchDeg = getDouble("scanner.Livox.mount.pitch_deg", 0);
        c.rollDeg  = getDouble("scanner.Livox.mount.roll_deg", 0);
        String sideStr = getStr("scanner.Livox.mount.side", "RIGHT");
        try { c.side = MountSide.valueOf(sideStr.trim().toUpperCase(Locale.ROOT)); }
        catch (Exception ignore) { c.side = MountSide.RIGHT; }

        // PLC-X 融合
        c.plcMergeEnabled = getBool("scanner.Livox.plc.merge.enabled", false);
        c.xLookupTolMs    = getInt("x.lookup.tolerance.ms", 30);

        c.manageXSourceLifecycle = false; // 由多机层托管
        return c;
    }

    /* ================= 读取 devices 列表（scanner.LivoxMulti.devices） ================= */
    @SuppressWarnings("unchecked")
    private List<Map<String, Object>> readDevicesList() {
        try {
            ResolvableType mapType  = ResolvableType.forClassWithGenerics(Map.class, String.class, Object.class);
            ResolvableType listType = ResolvableType.forClassWithGenerics(List.class, mapType);
            Bindable<List<Map<String, Object>>> target = Bindable.of(listType);
            return Binder.get(env)
                    .bind("scanner.LivoxMulti.devices", target)
                    .orElseGet(Collections::emptyList);
        } catch (Exception e) {
            log.debug("[MultiLivox] bind devices failed: {}", e.toString());
            return Collections.emptyList();
        }
    }

    /* ================= 设备级覆盖 & ROI ================= */
    private Roi parseDeviceRoi(Map<String,Object> dev){
        Roi r = new Roi();
        Double x0 = getDoubleOrNull(dev, "roi.x0", getDoubleOrNull(dev, "x0", null));
        Double x1 = getDoubleOrNull(dev, "roi.x1", getDoubleOrNull(dev, "x1", null));
        Double y0 = getDoubleOrNull(dev, "roi.y0", getDoubleOrNull(dev, "y0", null));
        Double y1 = getDoubleOrNull(dev, "roi.y1", getDoubleOrNull(dev, "y1", null));
        Double z0 = getDoubleOrNull(dev, "roi.z0", getDoubleOrNull(dev, "z0", null));
        Double z1 = getDoubleOrNull(dev, "roi.z1", getDoubleOrNull(dev, "z1", null));
        if (x0!=null && x1!=null && x1>=x0){ r.x0=x0; r.x1=x1; r.hasX=true; }
        if (y0!=null && y1!=null && y1>=y0){ r.y0=y0; r.y1=y1; r.hasY=true; }
        if (z0!=null && z1!=null && z1>=z0){ r.z0=z0; r.z1=z1; r.hasZ=true; }
        return r;
    }

    private void applyDeviceOverrides(LivoxConfig c, Map<String, Object> dev) {
        c.udpPort      = getInt(dev, "udp.port",     getInt(dev, "udpPort",      c.udpPort));
        c.recvBufBytes = Math.max(getInt(dev, "udp.recv_buf_bytes", getInt(dev,"recvBufBytes", c.recvBufBytes)), 1<<20);

        c.offDotNum   = getInt(dev, "offset.dot_num",   getInt(dev,"offDotNum",   c.offDotNum));
        c.offDataType = getInt(dev, "offset.data_type", getInt(dev,"offDataType", c.offDataType));
        c.headerBytes = getInt(dev, "header_bytes",     getInt(dev,"headerBytes", c.headerBytes));

        c.strideType1 = getInt(dev, "pt.stride.type1",  getInt(dev,"strideType1", c.strideType1));
        c.strideType2 = getInt(dev, "pt.stride.type2",  getInt(dev,"strideType2", c.strideType2));
        c.skipAfter1  = getInt(dev, "pt.skip_after_xyz.type1", getInt(dev,"skipAfter1", c.skipAfter1));
        c.skipAfter2  = getInt(dev, "pt.skip_after_xyz.type2", getInt(dev,"skipAfter2", c.skipAfter2));

        c.debugHexdump = getBool(dev, "debugHexdump", getBool(dev, "debug.hexdump", c.debugHexdump));

        // 兜底过滤（Y/Z 可能已被 ROI 覆盖）
        c.zMin = getDouble(dev, "filter.z_min", getDouble(dev,"zMin", c.zMin));
        c.zMax = getDouble(dev, "filter.z_max", getDouble(dev,"zMax", c.zMax));
        c.yMin = getDouble(dev, "filter.y_min", getDouble(dev,"yMin", c.yMin));
        c.yMax = getDouble(dev, "filter.y_max", getDouble(dev,"yMax", c.yMax));

        // 外参/偏移/符号
        c.h0      = getDouble(dev, "h0",       c.h0);
        c.xOffset = getDouble(dev, "x_offset", getDouble(dev,"xOffset", c.xOffset));
        c.yOffset = getDouble(dev, "y_offset", getDouble(dev,"yOffset", c.yOffset));
        c.sX      = getInt(dev, "sx", getInt(dev,"sX", c.sX));
        c.sY      = getInt(dev, "sy", getInt(dev,"sY", c.sY));
        c.sZ      = getInt(dev, "sz", getInt(dev,"sZ", c.sZ));

        // coord 覆盖（逐台）
        c.coordEnabled = getBool(dev, "coord.enabled", c.coordEnabled);
        c.xScale  = getDouble(dev, "coord.x_scale", c.xScale);
        c.yScale  = getDouble(dev, "coord.y_scale", c.yScale);
        c.zScale  = getDouble(dev, "coord.z_scale", c.zScale);
        c.invertX = getBool(dev, "coord.invert_x", c.invertX);
        c.invertY = getBool(dev, "coord.invert_y", c.invertY);
        c.invertZ = getBool(dev, "coord.invert_z", c.invertZ);
        c.yBase   = getDouble(dev, "coord.y_base",  c.yBase);
        c.zOffset = getDouble(dev, "coord.z_offset",c.zOffset);

        // 安装姿态/侧别
        String sideStr = getStr(dev, "mount.side", null);
        if (sideStr != null) {
            try { c.side = MountSide.valueOf(sideStr.trim().toUpperCase(Locale.ROOT)); }
            catch (Exception ignore) {}
        }
        c.yawDeg   = getDouble(dev, "mount.yaw_deg",   c.yawDeg);
        c.pitchDeg = getDouble(dev, "mount.pitch_deg", c.pitchDeg);
        c.rollDeg  = getDouble(dev, "mount.roll_deg",  c.rollDeg);

        // PLC-X 融合
        c.plcMergeEnabled = getBool(dev, "plc.merge.enabled",
                getBool(dev, "plcMergeEnabled", c.plcMergeEnabled));
        c.xLookupTolMs = getInt(dev, "x.lookup.tolerance.ms",
                getInt(dev, "xLookupTolMs", c.xLookupTolMs));
    }

    private static LivoxConfig cloneCfg(LivoxConfig o) {
        LivoxConfig c = new LivoxConfig();
        c.udpPort=o.udpPort; c.recvBufBytes=o.recvBufBytes;
        c.offDotNum=o.offDotNum; c.offDataType=o.offDataType; c.headerBytes=o.headerBytes;
        c.strideType1=o.strideType1; c.strideType2=o.strideType2; c.skipAfter1=o.skipAfter1; c.skipAfter2=o.skipAfter2;
        c.debugHexdump=o.debugHexdump;
        c.zMin=o.zMin; c.zMax=o.zMax; c.yMin=o.yMin; c.yMax=o.yMax;

        c.coordEnabled=o.coordEnabled;
        c.xScale=o.xScale; c.yScale=o.yScale; c.zScale=o.zScale;
        c.invertX=o.invertX; c.invertY=o.invertY; c.invertZ=o.invertZ;
        c.yBase=o.yBase; c.zOffset=o.zOffset;

        c.h0=o.h0; c.xOffset=o.xOffset; c.yOffset=o.yOffset;
        c.sX=o.sX; c.sY=o.sY; c.sZ=o.sZ;

        c.yawDeg=o.yawDeg; c.pitchDeg=o.pitchDeg; c.rollDeg=o.rollDeg;
        c.side=o.side; c.plcMergeEnabled=o.plcMergeEnabled;

        c.xLookupTolMs=o.xLookupTolMs; c.manageXSourceLifecycle=o.manageXSourceLifecycle;
        return c;
    }

    /* ================== 工具（Environment + Map） ================== */
    private String getStr(String key, String def) {
        String v = env.getProperty(key);
        return (v == null) ? def : v;
    }
    private int getInt(String key, int def) {
        String v = env.getProperty(key);
        if (v == null) return def;
        try { return Integer.parseInt(v.trim()); } catch (Exception e) { return def; }
    }
    private double getDouble(String key, double def) {
        String v = env.getProperty(key);
        if (v == null) return def;
        try { return Double.parseDouble(v.trim()); } catch (Exception e) { return def; }
    }
    private boolean getBool(String key, boolean def) {
        String v = env.getProperty(key);
        if (v == null) return def;
        return "true".equalsIgnoreCase(v) || "1".equals(v);
    }

    @SuppressWarnings("unchecked")
    private static Object getNested(Map<String, Object> m, String dotted) {
        if (m == null || dotted == null) return null;
        String[] parts = dotted.split("\\.");
        Object cur = m;
        for (String p : parts) {
            if (!(cur instanceof Map)) return null;
            cur = ((Map<String, Object>) cur).get(p);
            if (cur == null) return null;
        }
        return cur;
    }
    private static String getStr(Map<String, Object> m, String dotted, String def) {
        Object o = getNested(m, dotted);
        return (o == null) ? def : String.valueOf(o);
    }
    private static int getInt(Map<String, Object> m, String dotted, int def) {
        Object o = getNested(m, dotted);
        if (o == null) return def;
        if (o instanceof Number) return ((Number) o).intValue();
        try { return Integer.parseInt(String.valueOf(o)); } catch (Exception e) { return def; }
    }
    private static double getDouble(Map<String, Object> m, String dotted, double def) {
        Object o = getNested(m, dotted);
        if (o == null) return def;
        if (o instanceof Number) return ((Number) o).doubleValue();
        try { return Double.parseDouble(String.valueOf(o)); } catch (Exception e) { return def; }
    }
    private static Double getDoubleOrNull(Map<String, Object> m, String dotted, Double alt) {
        Object o = getNested(m, dotted);
        if (o == null) return alt;
        if (o instanceof Number) return ((Number) o).doubleValue();
        try { return Double.parseDouble(String.valueOf(o)); } catch (Exception e) { return alt; }
    }
    private static boolean getBool(Map<String, Object> m, String dotted, boolean def) {
        Object o = getNested(m, dotted);
        if (o == null) return def;
        if (o instanceof Boolean) return (Boolean) o;
        String s = String.valueOf(o);
        if ("true".equalsIgnoreCase(s) || "1".equals(s)) return true;
        if ("false".equalsIgnoreCase(s) || "0".equals(s)) return false;
        return def;
    }

    /* ================= 反射兜底取 ScanFrame 数据 ================= */
    private static long tryGetHostNs(ScanFrame f) {
        try {
            Method m = f.getClass().getMethod("getHostNs");
            Object v = m.invoke(f);
            if (v instanceof Number) return ((Number) v).longValue();
        } catch (Throwable ignore) {}
        try {
            Field fld = f.getClass().getDeclaredField("hostNs");
            fld.setAccessible(true);
            Object v = fld.get(f);
            if (v instanceof Number) return ((Number) v).longValue();
        } catch (Throwable ignore) {}
        return System.nanoTime();
    }
    private static double[] getArray(ScanFrame f, String fallbackFieldName, String... getters) {
        for (String g : getters) {
            try {
                Method m = f.getClass().getMethod(g);
                Object v = m.invoke(f);
                if (v instanceof double[]) return (double[]) v;
            } catch (Throwable ignore) {}
        }
        String[] cand = {fallbackFieldName, fallbackFieldName.toLowerCase(), fallbackFieldName+"w"};
        for (String name : cand) {
            try {
                Field fld = f.getClass().getDeclaredField(name);
                fld.setAccessible(true);
                Object v = fld.get(f);
                if (v instanceof double[]) return (double[]) v;
            } catch (Throwable ignore) {}
        }
        return null;
    }
}
